/*******************************************************************************

  Pilot Intelligence Library
    http://www.pilotintelligence.com/

  ----------------------------------------------------------------------------

    This program is free software: you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation, either version 3 of the License, or
    any later version.

    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
    GNU General Public License for more details.

    You should have received a copy of the GNU General Public License
    along with this program. If not, see <http://www.gnu.org/licenses/>.

*******************************************************************************/

#ifndef __UAS_H__
#define __UAS_H__

#include <stdint.h>
#include <stdio.h>
#include <stdlib.h>

#include <vector>
#include <map>
#include <list>
#include <string>
#include <tr1/functional>

#include <base/time/DateTime.h>
#include <hardware/Gps/POS_reader.h>

#include "UAS_types.h"
#include "utils_mavlink.h"



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

class UAS_Manager;

// Mavlink message call back function
typedef std::tr1::function<int (mavlink_message_t*)> Mavlink_Message_Handle;

typedef std::map<std::string, std::string> String2StringMap;


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

class UAS_Base
{
public:
    UAS_Base();
    virtual ~UAS_Base();

    virtual void init(void);
    virtual void release(void);

public:
    // basic information
    UAS_Type            uasType;                ///< UAS type


    uint8_t             ID, compID;             ///< mavlink ID number
    uint8_t             gcsID, gcsCompID;       ///< GCS ID & Component ID

    uint8_t             baseMode;               ///< base mode
    uint8_t             customMode;             ///< custom mode (flight mode)
    std::string         szFlightMode;           ///< flight mode string
    uint8_t             motorArmed;             ///< motor armed or not
    std::string         szMotorArmed;           ///< motor armed (string)

    uint8_t             uavType;                ///< UAV type
    uint8_t             autopilotType;          ///< Autopilot type
    uint8_t             systemStatus;           ///< System Status
    std::string         szSystemStatus;         ///< System Status string
    uint8_t             mavlinkVersion;         ///< mavlink version
    int                 mavlinkChan;            ///< mavlink channel

    int                 severity;               ///< Severity of status. Relies on the definitions
                                                ///<    within RFC-5424. See enum MAV_SEVERITY.
    char                statusText[256];        ///< system status text

    float               cpuLoad;                ///< CPU load
    float               battVolt;               ///< battery voltage
    float               battCurrent;            ///< battery current
    float               battRemaining;          ///< remaining battery
    float               commDropRate;           ///< communication drop rate

    uint64_t            systimeUnix;            ///< Timestamp of the master clock in
                                                ///<    microseconds since UNIX epoch.
    uint64_t            bootTime;               ///< Timestamp of the component clock since
                                                ///<    boot time in milliseconds.
    uint64_t            bootTime_begTime;       ///< local time when boot time begin
    uint64_t            bootTime_begTS;         ///< boot time begin time stamp
    uint64_t            bootTime_last;          ///< last Date-time bootTime


    // navigation data
    uint64_t            gpsTime;                ///< GPS time
    double              lat, lon, alt;          ///< GPS raw position
    double              HDOP_h, HDOP_v;         ///< HDOP
    double              gpsGroundSpeed;         ///< ground speed
    int                 gpsFixType;             ///< GPS fix type
    int                 nSat;                   ///< visable satellite

    double              gpLat, gpLon;           ///< fused position
    double              gpAlt, gpH;             ///< altitude / height
    double              gpVx, gpVy, gpVz;       ///< speed
    double              gpHeading;              ///< heading
    pi::DateTime        gpDateTime;             ///< fused position Date/time

    float               roll, pitch, yaw;       ///< attitude
    float               rollSpd,
                        pitchSpd,
                        yawSpd;

    double              homeLat, homeLng,       ///< Home position
                        homeAlt, homeH;         ///< Home altitude & height
    int                 homeSetCount;           ///< Home set count
    int                 homeSetted;             ///< Home position is setted
    int                 homePosReaded;          ///< Home position readed

    uint64_t            lastTM;
    double              lastAlt,
                        lastLat, lastLon;
    double              velV;                   ///< velocity in vertical
    double              velH;                   ///< velocity in horiz
    double              dis, disLOS;            ///< distance

    // raw sensor data
    int                 Ax, Ay, Az;
    int                 Gx, Gy, Gz;
    int                 Mx, My, Mz;
    int                 Ax_raw, Ay_raw, Az_raw;
    int                 Gx_raw, Gy_raw, Gz_raw;
    int                 Mx_raw, My_raw, Mz_raw;

    // RC data
    int                 rcRSSI;
    int                 rcRaw[8];
    int                 rcAll[16];
    int                 rcRaw_port;
    int                 rcAll_channels;

    // parameters
    AP_ParamArray       m_paramArray;           ///< MAV parameter array

    // waypoints
    AP_MissionArray     m_waypoints;            ///< waypoints
    int                 currMission;            ///< current mission

    // Stream request frequence
    int                 m_bStreamRequested;     ///< stream frequency has setted
    int                 *m_frqStream[16];       ///< request stream frequency


    // UAS manager
    UAS_Manager         *m_uasManager;

    // POS data manager
    pi::POS_DataManager m_posData;

    // Mavlink message handle list
    typedef std::map<std::string, Mavlink_Message_Handle> MavlinkMessageHandleMap;
    MavlinkMessageHandleMap     m_mavlinkMsgHandleMap;

public:
    int link_connected(void);
    int clear_home(void);

    int requireParameters(int clearOld=0);
    int requireParameter(int index);
    int requireParameter(char *id);

    int updateParameters(void);
    int updateParameter(int index);
    int updateParameter(char *id);


    int writeWaypoints(AP_MissionArray &wpa);
    int readWaypoints(void);
    int clearWaypoints(void);
    int setCurrentWaypoint(int idx);

    int writeWaypointsNum(void);
    int readWaypointsNum(void);

    int getStreamFrequency(MAV_DATA_STREAM streamID);
    int setStreamFrequency(MAV_DATA_STREAM streamID, int freq=-1);


    int executeCommand(MAV_CMD command, int confirmation,
                       float param1, float param2, float param3, float param4,
                       float param5, float param6, float param7,
                       int component);
    int executeCommandAck(int num, bool success);

    int _MAV_CMD_DO_SET_SERVO(int ch, int pwm);


    int registMavlinkMessageHandle(const std::string &handleName, Mavlink_Message_Handle &msgHandle);
    int unregistMavlinkMessageHandle(const std::string &handleName);

    virtual int startTimer(void);
    virtual int stopTimer(void);

    virtual int gen_listmap_important(String2StringMap &lm);
    virtual int gen_listmap_all(String2StringMap &lm);

    virtual int parseMavlinkMsg(mavlink_message_t *msg);
    virtual int sendMavlinkMsg(mavlink_message_t *msg);
    virtual int timerFunction(void *arg);

protected:
    virtual int setPosData(void);

protected:
    pi::TimerTask                   m_timer;
    pi::Mutex                       m_mutexMessageHandle;

    uint64_t                        m_timerCount;
    int                             m_connectTime;
    int                             m_paramAutoLoaded;

    int                             m_bLinkConnected;                   /// datalink ok?
    int                             m_pkgLost, m_pkgLastID;
    int                             m_recvMessageInSec;
    int                             m_statusMsgTime;

    int                             m_bCmdConfirmed;
    char                            m_sLastCommand[256];

    ValueAverager<float>            m_avgBatV;

    int                             m_tsValid_Year;
    uint64_t                        m_tsLastPOS;
};


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

class UAS_MAV : public UAS_Base
{
public:

    ///< auto flight stage
    enum AutoFlight_Stage {
        AF_NONE,                            ///< none, no autoflight task
        AF_CANCEL,                          ///< cancel autoflight, change flight mode to guided
        AF_CHANGE_MODE,                     ///< change flight mode
        AF_ARM,                             ///< arm
        AF_TAKEOFF,                         ///< takeoff
        AF_START_MISSION                    ///< start mission
    };

    ///< circular flight stage
    enum CircularFlight_Stage {
        CF_NONE,                            ///< none, no circular flight
        CF_REACH_CIRCLE,                    ///< approach circular
        CF_DO_CIRCLE                        ///< moving in circle
    };

    ///< Guided control source
    enum GuidedControlSource {
        GCS_MOUSE               = 0,        ///< control source is mouse
        GCS_JOYSTICK            = 1,        ///< control source is joystick
        GCS_FOLLOWME            = 2,        ///< control source is mobile GPS
        GCS_CIRCULARFLIGHT      = 3         ///< control source is CircularFlight
    };

public:
    UAS_MAV();
    virtual ~UAS_MAV();

    virtual void init(void);
    virtual void release(void);

    virtual int parseMavlinkMsg(mavlink_message_t *msg);
    virtual int timerFunction(void *arg);

    virtual int gen_listmap_important(String2StringMap &lm);
    virtual int gen_listmap_all(String2StringMap &lm);


    // landing gear
    virtual int landingGearDown(int nTry=3);
    virtual int landingGearUp(int nTry=3);

    // guided flight
    virtual int setGuidedTarget(double Lat, double Lng, double Alt,
                                GuidedControlSource controlSource, int forceSet=0);
    virtual int moveTo(double Lat, double Lng, double Alt);
    virtual int setTargetGlobal(double Lat, double Lng, double Alt);
    virtual int changeHeight(double newH);

    // automatic flight
    virtual int doAutoFlight(void);
    virtual int stopAutoFlight(void);
    virtual std::string getAutoFlightStage(void);

    // circular flight
    virtual int doCircularFlight(double Lat, double Lng, double R);
    virtual int stopCircularFlight(void);
    virtual CircularFlight_Stage getCircularFlightStage(void);

public:

    // use mouse or joystick control flight
    struct GuidedControl {
        double                      tmLastAction;           ///< Last timestamp of sending guide target
        double                      tmLastInput;            ///< Last timestamp of receive user input

        double                      lat, lng, alt;          ///< Guided target position/alt
        int                         posSetted;              ///< postion has been setted

        int                         enable;                 ///< GuidedFlight enable/disable
        GuidedControlSource         controlSource;          ///< control source


        GuidedControl() {
            tmLastAction = 0;
            tmLastInput = 0;

            posSetted = 0;

            lat = 0;
            lng = 0;
            alt = 0;

            enable = 0;
            controlSource = GCS_MOUSE;
        }

        void setPosition(double Lat, double Lng, double Alt) {
            lat = Lat;
            lng = Lng;
            alt = Alt;

            posSetted = 1;
        }

        double Lat(void) const { return lat; }
        double Lng(void) const { return lng; }
        double Alt(void) const { return alt; }

        void turnOn(void) { enable = 1; }
        void turnOff(void) { enable = 0; }
        int isEnabled(void) const { return enable; }
    } m_guidedControl;

protected:
    // automatic flight data
    AutoFlight_Stage                m_autoFlightStage;
    double                          m_tmAutoFlight_CMD;
    int                             m_nAutoFlight_Try;

    // circular flight data
    struct CircularFlightData {
        CircularFlight_Stage        stage;                          ///< current stage

        double                      centerLng;                      ///< circle center lng
        double                      centerLat;                      ///< circle center lat
        double                      radius;                         ///< circle radius

        double                      tmLastAction;                   ///< last action timestamp
        double                      speedH;                         ///< horiztonal speed

        CircularFlightData() {
            stage = CF_NONE;
        }
    } m_circularFlightData;

    // automatic landing gear
    struct AutoLandingGear {
        double                      lastH;                          ///< last height (in m, above ground)
        double                      tmLast;                         ///< previous timestamp (in second)
        double                      spdDown;                        ///< download speed

        AutoLandingGear() {
            lastH = 0;
            tmLast = 0;
        }
    } m_autoLandingGearData;

    uint64_t                offboardTimeStamp;
    bool                    offboardFlag;
    mavlink_message_t       offboardMsg;    ///< for saving offboard setPoint stream


    // internal used control function
    virtual int _doAutoFlight(void);
    virtual int _doCircularFlight(void);
    virtual int _doAutoLandingGear(void);
    virtual int _doSetTargetGlobal(void);
};


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

class UAS_GCS : public UAS_Base
{
public:
    UAS_GCS();
    virtual ~UAS_GCS();

    virtual void init(void);
    virtual void release(void);

    virtual int parseMavlinkMsg(mavlink_message_t *msg);
    virtual int gen_listmap_important(String2StringMap &lm);
    virtual int gen_listmap_all(String2StringMap &lm);

    virtual int timerFunction(void *arg);


    int sendOptions(uint8_t optType,
                    int32_t parm1=0, int32_t parm2=0, int32_t parm3=0,
                    uint16_t parm4=0, uint16_t parm5=0, uint16_t parm6=0, uint16_t parm7=0);
    int send_ATA_yawOffset(void);

public:
    double          ataYawOffset;
    int             ataYawOffset_setted;
};


////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

class UAS_Telem : public UAS_Base
{
public:
    UAS_Telem();
    virtual ~UAS_Telem();

    virtual void init(void);
    virtual void release(void);

    virtual int parseMavlinkMsg(mavlink_message_t *msg);
    virtual int gen_listmap_important(String2StringMap &lm);
    virtual int gen_listmap_all(String2StringMap &lm);

    virtual int timerFunction(void *arg);

public:
    // Telemetry data
    int                 radioRSSI;
    int                 radioRSSI_remote;

    float               radioRSSI_per, radioRSSI_remote_per;

    int                 radioRX_errors;
    int                 radioFixed;
    int                 radioTXBuf;
    int                 radioNoise;
    int                 radioNoise_remote;
};



////////////////////////////////////////////////////////////////////////////////
////////////////////////////////////////////////////////////////////////////////

typedef std::vector<UAS_Base*>   UAS_Array;
typedef std::map<int, UAS_Base*> UAS_IDMap;

class UAS_Manager
{
public:
    UAS_Manager();
    virtual ~UAS_Manager();

    virtual void init(void);
    virtual void release(void);
    virtual void reset(void);

    int startTimer(void);
    int stopTimer(void);
    virtual int timerFunction(void *arg);

    virtual int parseMavlinkMsg(mavlink_message_t *msg);
    virtual int sendMavlinkMsg(mavlink_message_t *msg);

    virtual int gen_listmap_important(String2StringMap &lm);
    virtual int gen_listmap_all(String2StringMap &lm);

    UAS_Array*      get_uas(void);
    UAS_Base*       get_uas(int id);
    int             get_uas(UAS_Type t, UAS_Array &uasArr);

    int             get_mav(UAS_Array &uasArr);
    int             get_gcs(UAS_Array &uasArr);
    int             get_telem(UAS_Array &uasArr);

    UAS_MAV*        get_active_mav(void);
    UAS_GCS*        get_active_gcs(void);
    UAS_Telem*      get_active_telem(void);

    UAS_MAV*        set_active_mav(int id);

    int put_msg_buff(uint8_t *buf, int len);
    int get_msg_buff(uint8_t *buf, int *len);

protected:
    UAS_Array               m_arrUAS;
    UAS_IDMap               m_mapUAS;

    pi::TimerTask           m_timer;
    uint64_t                m_timerCount;
    pi::Mutex               m_mutexMsgWrite;
    std::vector<uint8_t>    m_msgBuffer;

    float                   bpsIn, bpsOut;
    uint64_t                m_nReadLast, m_nWriteLast, m_tLast;

    UAS_Base                *m_activeMAV;
    UAS_Base                *m_activeGCS;
    UAS_Base                *m_activeTelem;

public:
    uint8_t                 gcsID, gcsCompID;       ///< GCS ID & Component ID
    int                     mavlinkChan;            ///< mavlink channel
};


#endif // end of __UAS_H__
